#ifndef _MY_DEFINE_
#define _MY_DEFINE_

//----------------------------------------------
//----------------------------------------------
//----------------------------------------------
//----------------------------------------------
									//DFA (run.c)
#define INIT_ROBOT				0
#define GET_FIELD_INFO			1
#define GET_SENSOR_VAL			2
#define GET_SENSOR_VAL_1		3
#define UPDATE_SENSOR_VAL		4
#define WAIT_TO_START			5

#define GO_TO_STOCK_ZONE		10
#define GO_TO_STOCK_ZONE_1		11
#define GO_TO_STOCK_ZONE_2		12

#define	PUT_WHITE_BLOCK			20
#define PUT_WHITE_BLOCK_1		21

#define GO_TO_GOLDEN_BLOCK		30
#define GO_TO_GOLDEN_BLOCK_1	31

#define GET_GOLDEN_BLOCK		35
#define GET_GOLDEN_BLOCK_2		36

//----------------------------------------------
//----------------------------------------------
//----------------------------------------------
//----------------------------------------------
									//switch input define
#define B_YELLOW	(0xFD)
#define B_GREEN		(0xFE)
#define B_ROSE		(0xFB)
#define B_BLUE		(0xF7)
#define B_HAND_UP	(0xDF)
#define B_HAND_DOWN	(0xEF)
//IN3
#define B_L1		127 //4
#define B_L2		191 //3
#define B_L3		223 //6
#define B_R1		239 //
#define B_R2		247 //
#define B_R3		251 //7


#define B_FRONT_L	223
#define B_FRONT_R	254
#define B_GIFT_BIG	127
#define B_GIFT_SMALL 191
//IN2
#define B_SENSOR_HIGH	253//1
#define B_SENSOR_LOW	0
//----------------------------------------------
//----------------------------------------------
//----------------------------------------------
//----------------------------------------------
									//real speed - PD function (speed.c)
#define Kp_LEFT		5				
#define Kd_LEFT		5
#define Kp_RIGHT	5
#define Kd_RIGHT	5
#define ERROR_MIN	20		//minimum error to calculate PD

//----------------------------------------------
//----------------------------------------------
//----------------------------------------------
//----------------------------------------------
#define ENC_COUNT_DIV	1			//nhan encoder moi~ (ENC_COUNT_DIV) xung 
									//	(gia tri fai > 0)
//----------------------------------------------
//----------------------------------------------
//----------------------------------------------
//----------------------------------------------
									//xu ly qua trang/vang (wheel.c)
#define GOLDEN_BLOCK_speed	dc3_speed
#define GOLDEN_BLOCK_SPD	150

#define WHITE_BLOCK_speed	dc5_speed
#define WHITE_BLOCK_SPD		200
//----------------------------------------------
									//test_forward_pd()

#define Div_P		5
#define Div_D		5

//PID const
#define 	K_PRO 			2			//* proportional constant */
#define 	K_DRV 			2			//* derivative constant */
#define 	TMAX 			100*256	//* maximum output value */ //ko vuot qua 127									
									//keep_stable()
#define			KEEP_STABLE_KP			1
#define			KEEP_STABLE_SPEED_MAX	30
#define			TIME_FOR_STABLE	 25
#define			SPEED_MAX		 250
#define			SPEED_MIN		 10
/* Mask value setting x: With Mask (Invalid) o:Without mask (Valid)     */
#define         MASK2_2         0x18    /* oox00xoo */
#define         MASK3_C         0x3c    /* ooxxxxoo */
#define         MASK1_1         0x18    /* oooxxooo */
#define         MASK3_3         0xe7    /* xx0000xx */
#define         MASK0_3         0x07    /* 00000xxx */
#define         MASK3_0         0xe0    /* xxx00000 */



#define OK				    0
#define LEFT_CENTER_OUT1	1	//00011100
#define	LEFT_OUT1			2
#define	LEFT_OUT2			3
#define	LEFT_OUT3			4
#define	RIGHT_CENTER_OUT1	11
#define	RIGHT_OUT1			12
#define	RIGHT_OUT2			13
#define	RIGHT_OUT3			14
//----------------------------------------------
#define			LEFT		0
#define			RIGHT		1
//----------------------------------------------
//----------------------------------------------
//----------------------------------------------- wheel_action
#define 	NO_THING		0
//Forward1
#define 	FORWARD1		1
#define 	FORWARD1_1		2
#define 	FORWARD1_2		3
#define 	FORWARD1_3		4
#define 	FORWARD1_31		131
#define 	FORWARD1_32		132
#define 	FORWARD1_33		133
#define 	FORWARD1_4		5
#define 	FORWARD1_41		141
#define 	FORWARD1_42		142
#define 	FORWARD1_43		143
#define 	FORWARD1_5		6
#define 	FORWARD1_6		7
#define 	FORWARD1_7		8
#define 	FORWARD1_8		9
#define 	FORWARD1_9		10
#define 	FORWARD2		200
#define 	FORWARD2_1		201
#define 	FORWARD2_2		202
#define 	FORWARD2_3		203
#define 	FORWARD2_4		204
#define 	FORWARD2_END	205

#define 	FORWARD3		1
#define 	FORWARD3_1		31
#define 	FORWARD3_2		32
#define 	FORWARD3_3		33
#define 	FORWARD3_31		331
#define 	FORWARD3_32		332
#define 	FORWARD3_33		333
#define 	FORWARD3_4		34
#define 	FORWARD3_41		341
#define 	FORWARD3_42		342
#define 	FORWARD3_43		343
#define 	FORWARD3_5		35
#define 	FORWARD3_6		36
#define 	FORWARD3_7		37
#define 	FORWARD3_8		38
#define 	FORWARD3_9		39
#define 	FORWARD3_10		310

#define		FORWARD_ENCODER	210
#define		FORWARD_ENCODER_1	211
#define		FORWARD_ENCODER_2	212
#define		FORWARD_ENCODER_3	213

#define		HAND_NOTHING	0
#define		HAND_UP			1
#define		HAND_UP_1		11
#define		HAND_UP_2		12
#define		HAND_DOWN		2
#define		HAND_DOWN_1		21

#define 		NO_GET_GIFT_SMALL 	3
#define 		NO_GET_GIFT_BIG 	4
#define		GET_GIFT_SMALL		5
#define		GET_GIFT_BIG		6

#define		SUB_NOTHING		0
#define		SUB_0			1
#define		SUB_1			2
#define		SUB_2			3
#define		SUB_3			4
#define		SUB_4			5
#define		SUB_5			6
#define		SUB_6			7

//turn left
#define 	TURNLEFT						11
#define 	TURNLEFT1						12
#define 	TURNLEFT1_1						212
#define 	TURNLEFT2						13
#define 	TURNLEFT_END					14
#define 	TURNLEFT3						15

//turn right
#define 	TURNRIGHT				11
#define 	TURNRIGHT1				12
#define 	TURNRIGHT1_1			212
#define 	TURNRIGHT2				13
#define 	TURNRIGHT_END			14
#define 	TURNRIGHT3				15
#define		TURNRIGHT4				33
#endif
